﻿using OpenCvSharp;
using System;
using System.Drawing;
using System.IO;
using System.Net.Sockets;
using System.Text;
using System.Threading;
using System.Timers;
using System.Windows;
using System.Windows.Media.Imaging;
using sl;
using System.Windows.Input;

namespace PlantPick
{


    /// <summary>
    /// MainWindow.xaml 的交互逻辑
    /// </summary>
    public partial class MainWindow : System.Windows.Window
    {
        private bool isConnectRobot = false;
        private bool isRobotWorking = false;
        private Socket socket = null;
        private bool isCapture = false;
        private Thread threadclient = null;
        private String cameraType = "Normal";
        private String zedView = "left";
        private int normalCameraDeepth = 50;

        private VideoCapture normalCamera = null;
        private OpenCvSharp.Mat normalImage = new OpenCvSharp.Mat();
        private Detection detection = new Detection();

        private Camera zedCamera = null;
        private sl.Mat zedMat = new sl.Mat();
        sl.Mat depth_map = new sl.Mat();
        private RuntimeParameters runtimeParameters = null;
        OpenCvSharp.Mat zedTransCv = null;
        
        OpenCvSharp.Mat frame = new OpenCvSharp.Mat();
        OpenCvSharp.Mat src = new OpenCvSharp.Mat();

        private WorldPointTrans worldPointTrans = new WorldPointTrans();
        public MainWindow()
        {
            InitializeComponent();
            System.Timers.Timer timer = new System.Timers.Timer();
            timer.Enabled = true;
            timer.Interval = 100;
            timer.Elapsed += renderCamera;
            timer.Start();
            NomalCameraDeepth.Text = "50";
        }

        public void renderCamera(object source, ElapsedEventArgs e) {
            
            if (isCapture)
            {
                try
                {
                    if (cameraType == "ZED")
                    {
                        if (zedCamera.Grab(ref runtimeParameters) == ERROR_CODE.SUCCESS)
                        {
                            if (zedView == "left")
                            {
                                zedCamera.RetrieveImage(zedMat, sl.VIEW.LEFT, sl.MEM.CPU);
                            }
                            else
                            {
                                zedCamera.RetrieveImage(zedMat, sl.VIEW.RIGHT, sl.MEM.CPU);
                            }

                            zedCamera.RetrieveMeasure(depth_map, MEASURE.XYZRGBA);
                            OpenCvSharp.Mat zedTransCv = new OpenCvSharp.Mat(zedMat.GetHeight(), zedMat.GetWidth(), OpenCvSharp.MatType.CV_8UC4, zedMat.GetPtr());
                            //frame = Cv2.ImRead("1.jpg");
                            //video.Read(frame);
                            OpenCvSharp.Mat prePro = detection.ImagePreProc(zedTransCv);
                            LineSegmentPoint point = detection.ImageRootDect(prePro, zedTransCv);

                            float4 xyz_value;
                            depth_map.GetValue((int)((point.P1.X + point.P2.X) / 2), (int)((point.P1.Y + point.P2.Y) / 2), out xyz_value, MEM.CPU);
                            //detection.ImageLeafDect(prePro, zedTransCv);
                            this.Dispatcher.BeginInvoke((Action)delegate ()
                            {
                                //mPositonX.Text = ((point.P1.X + point.P2.X) / 2).ToString();
                                //mPositonY.Text = ((point.P1.Y + point.P2.Y) / 2).ToString();
                                mPositonX.Text = xyz_value.x.ToString();
                                mPositonY.Text = xyz_value.y.ToString();
                                mPositonZ.Text = xyz_value.z.ToString();
                                mCameraView.Source = MatToBitmapImage(zedTransCv);
                            });

                        }
                        else {
                            log("获取图片失败，请检查相机" + zedCamera.Grab(ref runtimeParameters).ToString());
                            closeCamere();
                            CameraSelect.IsEnabled = true;
                            return;
                        }
                    }
                    else if (cameraType == "Normal")
                    {
                        bool r = normalCamera.Read(normalImage);
                        if (!r) {
                            log("获取图片失败，请检查相机");
                            closeCamere();
                            normalCamera.Release();
                            return;
                        }
                        OpenCvSharp.Mat prePro = detection.ImagePreProc(normalImage);
                        LineSegmentPoint point = detection.ImageRootDect(prePro, normalImage);
                        this.Dispatcher.BeginInvoke((Action)delegate ()
                        {
                            float x = ((point.P1.X + point.P2.X) / 2);
                            float y = ((point.P1.X + point.P2.X) / 2);
                            Point3d real = worldPointTrans.getWorldPoints(new Point2f(x, y), WorldPointTrans.matRvec, WorldPointTrans.mattvec, WorldPointTrans.matcameraMatrix);
                            mPositonX.Text = real.X.ToString();
                            mPositonY.Text = real.Y.ToString();
                            mPositonZ.Text = real.Z.ToString();
                            mCameraView.Source = MatToBitmapImage(normalImage);
                        });
                    }
                } catch (Exception ex) {
                    closeCamere();
                    log(ex.ToString());

                    log("读取图片失败，请尝试重新打开相机");
                }
            

            }
        }
        private void closeCamere() {
            isCapture = false;
            this.Dispatcher.BeginInvoke((Action)delegate ()
            {
                CameraSelect.IsEnabled = true;
            });
        }
        private void OpenCamenra(object sender, RoutedEventArgs e)
        {
            if (isCapture)
            {
                isCapture = !isCapture;
                if (cameraType == "ZED")
                {
                    log("关闭ZED相机");
                    CameraSelect.IsEnabled = true;
                    zedCamera.Close();
                }
                else
                {
                    log("关闭相机");
                    CameraSelect.IsEnabled = true;
                    normalCamera.Release();
                }
                
            }
            else
            {
                if (cameraType == "ZED")
                {
                    sl.InitParameters init_params = new sl.InitParameters();

                    init_params.cameraFPS = 30;
                    //init_params.depthMode = DEPTH_MODE.ULTRA;
                    //init_params.coordinateUnits = UNIT.METER;
                    //init_params.coordinateSystem = COORDINATE_SYSTEM.RIGHT_HANDED_Y_UP;
                    //init_params.depthMaximumDistance = 15f;
                    //init_params.resolution = RESOLUTION.HD1080;
                    init_params.resolution = RESOLUTION.HD720;
                    init_params.cameraFPS = 30;
                    init_params.coordinateUnits = UNIT.METER;
                    init_params.depthMode = DEPTH_MODE.PERFORMANCE;
                    zedCamera = new sl.Camera(0);
                    sl.ERROR_CODE err = zedCamera.Open(ref init_params);
                    if (err != sl.ERROR_CODE.SUCCESS)
                    {
                        log("打开ZED相机失败，请确认是否安装cuda sdk 10.02及zed sdk，并保证相机连接");
                        isCapture = false;
                        CameraSelect.IsEnabled = true;
                        return;
                    }

                    int Height = zedCamera.ImageHeight;
                    int Width = zedCamera.ImageWidth;
                    depth_map.Create((uint)Width, (uint)Height, MAT_TYPE.MAT_32F_C1, MEM.CPU);
                    Resolution res = new Resolution((uint)Width, (uint)Height);
                    zedMat.Create(res, MAT_TYPE.MAT_8U_C4, MEM.CPU);
                    zedTransCv = new OpenCvSharp.Mat(zedMat.GetHeight(), zedMat.GetWidth(), MatType.CV_8UC3);
                    runtimeParameters = new RuntimeParameters();
                    runtimeParameters.sensingMode = SENSING_MODE.FILL;
                    if (zedCamera.Grab(ref runtimeParameters) == ERROR_CODE.SUCCESS) {
                        isCapture = true;
                        CameraSelect.IsEnabled = false;
                        log("打开ZED相机成功");
                    }

                }
                else
                {
                    normalCamera = new VideoCapture(0);
                    bool r = normalCamera.Read(normalImage);
                    if (r)
                    {
                        isCapture = true;
                        log("打开相机成功");
                        CameraSelect.IsEnabled = false;
                    }
                    else
                    {
                        isCapture = false;
                        CameraSelect.IsEnabled = true;
                        log("打开相机失败");
                    }
                }
            }



        }

        public BitmapImage MatToBitmapImage(OpenCvSharp.Mat image)
        {
            Bitmap bitmap = OpenCvSharp.Extensions.BitmapConverter.ToBitmap(image);
            using (MemoryStream stream = new MemoryStream())
            {
                bitmap.Save(stream, System.Drawing.Imaging.ImageFormat.Png); // 坑点：格式选Bmp时，不带透明度

                stream.Position = 0;
                BitmapImage result = new BitmapImage();
                result.BeginInit();
                result.CacheOption = BitmapCacheOption.OnLoad;
                result.StreamSource = stream;
                result.EndInit();
                result.Freeze();
                return result;
            }
        }

        private void connectRobot(object sender, RoutedEventArgs e)
        {
            if (!isConnectRobot)
            {
                try
                {
                    //客户端套接字连接到网络节点上，用的是Connect
                    socket = new Socket(AddressFamily.InterNetwork, SocketType.Stream, ProtocolType.Tcp);
                    log("开始连接机器人"+"\n");
                    //IPEndPoint point = new IPEndPoint(IPAddress.Parse("192.168.43.1"), 8081);// comboBox1IP
                    //socket.Connect(point);
                    //isConnectRobot = !isConnectRobot;
                    //threadclient = new Thread(Recv);
                    //threadclient.IsBackground = true;
                    //threadclient.Start();
                    mConnectRobot.Content = "断开机器人" ;
                    isConnectRobot = !isConnectRobot;
                }
                catch (Exception)
                {
                    log("连接失败");
                    return;
                }

                return;
            }
            //将获取的IP地址和端口号绑定在网络节点上

            if (isConnectRobot)
            {
                mConnectRobot.Content = "连接机器人";
                log("断开机器人\n");
                //socket.Shutdown(SocketShutdown.Both);
                //threadclient.Interrupt();
                //socket.Close();
                // button2.Enabled = false;
                isConnectRobot = !isConnectRobot;
                return;
            }
        }

        private void Recv()//网络接收数据
        {
            while (true)//持续监听服务端发来的消息
            {
                try
                {

                    //定义一个1M的内存缓冲区，用于临时性存储接收到的消息
                    byte[] arrRecvmsg = new byte[1024];
                    //将客户端套接字接收到的数据存入内存缓冲区
                    socket.Receive(arrRecvmsg);
                    //将套接字获取到的字符数组转换为人可以看懂的字符串
                    string strSRecMsg = Encoding.Default.GetString(arrRecvmsg);
                    log(strSRecMsg);

                }
                catch (Exception)
                {

                }
            }
        }

        private void log(String i) {
            this.Dispatcher.BeginInvoke((Action)delegate ()
            {
                mLogView.AppendText(i.TrimEnd() + "\n");
                if (!mLogView.IsFocused)
                {
                    mLogView.ScrollToEnd();
                }
            });
            
        }

        public void ClientSendMsg(string sendMsg) //发送数据
        {
            log("下发命令"+sendMsg);
            byte[] arrClientSendMsg = Encoding.Default.GetBytes(sendMsg);
            //调用客户端套接字发送字节数组 
            //socket.Send(arrClientSendMsg);
        }

        /// <summary>
        /// Base64加密
        /// </summary>
        /// <param name="encodeType">加密采用的编码方式</param>
        /// <param name="source">待加密的明文</param>
        /// <returns></returns>
        public static string Base64Encode(Encoding encodeType, string source)
        {
            string encode = string.Empty;
            byte[] bytes = encodeType.GetBytes(source);
            try
            {
                encode = Convert.ToBase64String(bytes);
            }
            catch
            {
                encode = source;
            }
            return encode;
        }

        /// <summary>
        /// Base64解密
        /// </summary>
        /// <param name="encodeType">解密采用的编码方式，注意和加密时采用的方式一致</param>
        /// <param name="result">待解密的密文</param>
        /// <returns>解密后的字符串</returns>
        public static string Base64Decode(Encoding encodeType, string result)
        {
            string decode = string.Empty;
            byte[] bytes = Convert.FromBase64String(result);
            try
            {
                decode = encodeType.GetString(bytes);
            }
            catch
            {
                decode = result;
            }
            return decode;
        }

        private void mCommandButton_Click(object sender, RoutedEventArgs e)
        {
            if (!isCapture)
            {
                log("请先打开相机");
                return;
            }
            if (!isConnectRobot)
            {
                log("请先连接机器人");
                return;
            }
            if (!isRobotWorking)
            {
                log("请先开启机器人");
                return;
            }
            String commandTemp = "GetPosition:" + mPositonX.Text + "," + mPositonX.Text + "," + mPositonZ.Text + "," + "100.0" +"\n";
            log(commandTemp);
            ClientSendMsg("#%" + Base64Encode(Encoding.UTF8, commandTemp) + "%#");
        }

        private void switchRobotState(object sender, RoutedEventArgs e)
        {
            if (!isCapture) {
                log("请先打开相机");
                return;
            }
            if (!isConnectRobot)
            {
                log("请先连接机器人");
                return;
            }
            if (isRobotWorking)
            {
                String commandTemp = "Ronlinesta:[0]";
                ClientSendMsg(commandTemp);
                mSwitchRobotState.Content = "离线";
                log("停止工作");
                isRobotWorking = !isRobotWorking;
                return;
            }
            else {
                String commandTemp = "Ronlinesta:[1]";
                ClientSendMsg(commandTemp);
                mSwitchRobotState.Content = "工作";
                log("开始工作");
                isRobotWorking = !isRobotWorking;
                return;
            }

        }

        private void VideoChanged(object sender, System.Windows.Controls.SelectionChangedEventArgs e)
        {
            if (isCapture) {
                log("请先关闭相机");
                return;
            }
            cameraType = CameraSelect.SelectedValue.ToString().Replace("System.Windows.Controls.ComboBoxItem: ","");
            if (cameraType == "ZED" && zedViewSelect != null && NomalCameraDeepth != null) {
                zedViewSelect.Visibility = Visibility.Visible;
                NomalCameraDeepth.Visibility = Visibility.Hidden;
            }
            if (cameraType == "Normal" && zedViewSelect != null && NomalCameraDeepth!=null)
            {
                zedViewSelect.Visibility = Visibility.Hidden;
                NomalCameraDeepth.Visibility = Visibility.Visible;
            }

        }

        private void NomalCameraDeepthChange(object sender, System.Windows.Controls.TextChangedEventArgs e)
        {
            if (NomalCameraDeepth.Text.Length > 0)
            {
                normalCameraDeepth = int.Parse(NomalCameraDeepth.Text.Trim());
                normalCameraDeepth = normalCameraDeepth > 100 ? 100 : normalCameraDeepth;
                normalCameraDeepth = normalCameraDeepth < 0 ? 0 : normalCameraDeepth;
                NomalCameraDeepth.Text = normalCameraDeepth.ToString();
            }


        }
        private void NomalCameraDeepthChange_KeyPress(object sender, KeyEventArgs e)
        {

            if (!((e.Key >= Key.D0 && e.Key <= Key.D9)             //大键盘0-9
                || (e.Key >= Key.NumPad0 && e.Key <= Key.NumPad9)   //小键盘0-9 
                || e.Key == Key.Delete || e.Key == Key.Enter
                || e.Key == Key.Back || e.Key == Key.Tab
                || e.Key == Key.Right || e.Key == Key.Left))
            {
                e.Handled = true;//不可输入
            }
        }

        private void zed_view_select_Click(object sender, RoutedEventArgs e)
        {
            if (zedViewSelect.Content.ToString() == "left")
            {
                zedView = "right";
                zedViewSelect.Content = "right";
            }
            else {
                zedView = "left";
                zedViewSelect.Content = "left";
            }
            
        }
    }
}
